//
// Created by fanghr on 18-3-28.
//

#include <iostream>
#include <iomanip>
#include <vector>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>

#include "../include/CameraCalibrator.h"

cv::Mat applyLookUp(const cv::Mat &image, const cv::MatND &lookup) {
    cv::Mat result(image.rows, image.cols, CV_8U);

    cv::Mat_<uchar>::iterator itr = result.begin<uchar>();
    cv::Mat_<uchar>::const_iterator it = image.begin<uchar>();
    cv::Mat_<uchar>::const_iterator itend = image.end<uchar>();
    for (; it != itend; ++it, ++itr) {

        *itr = lookup.at<uchar>(*it);
    }
    return result;
}

int main() {
    cv::namedWindow("Board Image");
    cv::Mat image;
    std::vector<std::string> filelist;

    for (int i = 1; i <= 9; i++) {

        std::stringstream str;
        str << "chessboards/right" << std::setw(2) << std::setfill('0') << i << ".jpg";
        std::cout << str.str() << std::endl;

        filelist.push_back(str.str());
        image = cv::imread(str.str(), 0);
        cv::imshow("Board Image", image);

        cv::waitKey(100);
    }

    CameraCalibrator cameraCalibrator;
    cv::Size boardSize(9, 6);
    cameraCalibrator.addChessboardPoints(
            filelist,
            boardSize, "Detected points");

    cameraCalibrator.calibrate(image.size());

    image = cv::imread(filelist[6], 0);
    cv::Mat uImage = cameraCalibrator.remap(image);

    cv::Mat cameraMatrix = cameraCalibrator.getCameraMatrix();
    std::cout << " Camera intrinsic: " << cameraMatrix.rows << "x" << cameraMatrix.cols << std::endl;
    std::cout << cameraMatrix.at<double>(0, 0) << " " << cameraMatrix.at<double>(0, 1) << " "
              << cameraMatrix.at<double>(0, 2) << std::endl;
    std::cout << cameraMatrix.at<double>(1, 0) << " " << cameraMatrix.at<double>(1, 1) << " "
              << cameraMatrix.at<double>(1, 2) << std::endl;
    std::cout << cameraMatrix.at<double>(2, 0) << " " << cameraMatrix.at<double>(2, 1) << " "
              << cameraMatrix.at<double>(2, 2) << std::endl;

    cv::namedWindow("Original Image");
    cv::imshow("Original Image", image);
    cv::namedWindow("Undistorted Image");
    cv::imshow("Undistorted Image", uImage);

    cv::waitKey();
    return 0;

    int dim(256);
    cv::Mat lut(1,
                &dim,
                CV_8U);

    for (int i = 0; i < 256; i++) {

        lut.at<uchar>(i) = 255 - i;
    }

    cv::namedWindow("Negative image");
    cv::imshow("Negative image", applyLookUp(image, lut));

    cv::waitKey();
    return 0;
}
